<!DOCTYPE html>
<html lang="en">
<head>
    <meta charset="utf-8">
    <title>src/equations/Equation.js - p2.js</title>
    <link rel="stylesheet" href="http://yui.yahooapis.com/3.9.1/build/cssgrids/cssgrids-min.css">
    <link rel="stylesheet" href="../assets/vendor/prettify/prettify-min.css">
    <link rel="stylesheet" href="../assets/css/main.css" id="site_styles">
    <link rel="icon" href="../assets/favicon.ico">
    <script src="http://yui.yahooapis.com/combo?3.9.1/build/yui/yui-min.js"></script>
</head>
<body class="yui3-skin-sam">

<div id="doc">
    <div id="hd" class="yui3-g header">
        <div class="yui3-u-3-4">
                <h1><img src="../assets/css/logo.png" title="p2.js" width="117" height="52"></h1>
        </div>
        <div class="yui3-u-1-4 version">
            <em>API Docs for: 0.7.1</em>
        </div>
    </div>
    <div id="bd" class="yui3-g">

        <div class="yui3-u-1-4">
            <div id="docs-sidebar" class="sidebar apidocs">
                <div id="api-list">
                    <h2 class="off-left">APIs</h2>
                    <div id="api-tabview" class="tabview">
                        <ul class="tabs">
                            <li><a href="#api-classes">Classes</a></li>
                            <li><a href="#api-modules">Modules</a></li>
                        </ul>
                
                        <div id="api-tabview-filter">
                            <input type="search" id="api-filter" placeholder="Type to filter APIs">
                        </div>
                
                        <div id="api-tabview-panel">
                            <ul id="api-classes" class="apis classes">
                                <li><a href="../classes/AABB.html">AABB</a></li>
                                <li><a href="../classes/AngleLockEquation.html">AngleLockEquation</a></li>
                                <li><a href="../classes/Body.html">Body</a></li>
                                <li><a href="../classes/Box.html">Box</a></li>
                                <li><a href="../classes/Broadphase.html">Broadphase</a></li>
                                <li><a href="../classes/Capsule.html">Capsule</a></li>
                                <li><a href="../classes/Circle.html">Circle</a></li>
                                <li><a href="../classes/Constraint.html">Constraint</a></li>
                                <li><a href="../classes/ContactEquation.html">ContactEquation</a></li>
                                <li><a href="../classes/ContactMaterial.html">ContactMaterial</a></li>
                                <li><a href="../classes/Convex.html">Convex</a></li>
                                <li><a href="../classes/DistanceConstraint.html">DistanceConstraint</a></li>
                                <li><a href="../classes/Equation.html">Equation</a></li>
                                <li><a href="../classes/EventEmitter.html">EventEmitter</a></li>
                                <li><a href="../classes/FrictionEquation.html">FrictionEquation</a></li>
                                <li><a href="../classes/GearConstraint.html">GearConstraint</a></li>
                                <li><a href="../classes/GSSolver.html">GSSolver</a></li>
                                <li><a href="../classes/Heightfield.html">Heightfield</a></li>
                                <li><a href="../classes/Island.html">Island</a></li>
                                <li><a href="../classes/IslandManager.html">IslandManager</a></li>
                                <li><a href="../classes/IslandNode.html">IslandNode</a></li>
                                <li><a href="../classes/Line.html">Line</a></li>
                                <li><a href="../classes/LinearSpring.html">LinearSpring</a></li>
                                <li><a href="../classes/LockConstraint.html">LockConstraint</a></li>
                                <li><a href="../classes/Material.html">Material</a></li>
                                <li><a href="../classes/NaiveBroadphase.html">NaiveBroadphase</a></li>
                                <li><a href="../classes/Narrowphase.html">Narrowphase</a></li>
                                <li><a href="../classes/Object pooling utility..html">Object pooling utility.</a></li>
                                <li><a href="../classes/OverlapKeeper.html">OverlapKeeper</a></li>
                                <li><a href="../classes/OverlapKeeperRecord.html">OverlapKeeperRecord</a></li>
                                <li><a href="../classes/Particle.html">Particle</a></li>
                                <li><a href="../classes/Plane.html">Plane</a></li>
                                <li><a href="../classes/PrismaticConstraint.html">PrismaticConstraint</a></li>
                                <li><a href="../classes/Ray.html">Ray</a></li>
                                <li><a href="../classes/RaycastResult.html">RaycastResult</a></li>
                                <li><a href="../classes/RevoluteConstraint.html">RevoluteConstraint</a></li>
                                <li><a href="../classes/RotationalLockEquation.html">RotationalLockEquation</a></li>
                                <li><a href="../classes/.html"></a></li>
                                <li><a href="../classes/RotationalSpring.html">RotationalSpring</a></li>
                                <li><a href="../classes/RotationalVelocityEquation.html">RotationalVelocityEquation</a></li>
                                <li><a href="../classes/SAPBroadphase.html">SAPBroadphase</a></li>
                                <li><a href="../classes/Shape.html">Shape</a></li>
                                <li><a href="../classes/Solver.html">Solver</a></li>
                                <li><a href="../classes/Spring.html">Spring</a></li>
                                <li><a href="../classes/TopDownVehicle.html">TopDownVehicle</a></li>
                                <li><a href="../classes/TupleDictionary.html">TupleDictionary</a></li>
                                <li><a href="../classes/Utils.html">Utils</a></li>
                                <li><a href="../classes/vec2.html">vec2</a></li>
                                <li><a href="../classes/WheelConstraint.html">WheelConstraint</a></li>
                                <li><a href="../classes/World.html">World</a></li>
                            </ul>
                
                            <ul id="api-modules" class="apis modules">
                            </ul>
                        </div>
                    </div>
                </div>
            </div>
        </div>
        <div class="yui3-u-3-4">
                <div id="api-options">
                    Show:
                    <label for="api-show-inherited">
                        <input type="checkbox" id="api-show-inherited" checked>
                        Inherited
                    </label>
            
                    <label for="api-show-protected">
                        <input type="checkbox" id="api-show-protected">
                        Protected
                    </label>
            
                    <label for="api-show-private">
                        <input type="checkbox" id="api-show-private">
                        Private
                    </label>
                    <label for="api-show-deprecated">
                        <input type="checkbox" id="api-show-deprecated">
                        Deprecated
                    </label>
            
                </div>
            
            <div class="apidocs">
                <div id="docs-main">
                    <div class="content">
<h1 class="file-heading">File: src/equations/Equation.js</h1>

<div class="file">
    <pre class="code prettyprint linenums">
module.exports = Equation;

var vec2 = require(&#x27;../math/vec2&#x27;),
    Utils = require(&#x27;../utils/Utils&#x27;),
    Body = require(&#x27;../objects/Body&#x27;);

/**
 * Base class for constraint equations.
 * @class Equation
 * @constructor
 * @param {Body} bodyA First body participating in the equation
 * @param {Body} bodyB Second body participating in the equation
 * @param {number} minForce Minimum force to apply. Default: -Number.MAX_VALUE
 * @param {number} maxForce Maximum force to apply. Default: Number.MAX_VALUE
 */
function Equation(bodyA, bodyB, minForce, maxForce){

    /**
     * Minimum force to apply when solving.
     * @property minForce
     * @type {Number}
     */
    this.minForce = typeof(minForce)===&quot;undefined&quot; ? -Number.MAX_VALUE : minForce;

    /**
     * Max force to apply when solving.
     * @property maxForce
     * @type {Number}
     */
    this.maxForce = typeof(maxForce)===&quot;undefined&quot; ? Number.MAX_VALUE : maxForce;

    /**
     * First body participating in the constraint
     * @property bodyA
     * @type {Body}
     */
    this.bodyA = bodyA;

    /**
     * Second body participating in the constraint
     * @property bodyB
     * @type {Body}
     */
    this.bodyB = bodyB;

    /**
     * The stiffness of this equation. Typically chosen to a large number (~1e7), but can be chosen somewhat freely to get a stable simulation.
     * @property stiffness
     * @type {Number}
     */
    this.stiffness = Equation.DEFAULT_STIFFNESS;

    /**
     * The number of time steps needed to stabilize the constraint equation. Typically between 3 and 5 time steps.
     * @property relaxation
     * @type {Number}
     */
    this.relaxation = Equation.DEFAULT_RELAXATION;

    /**
     * The Jacobian entry of this equation. 6 numbers, 3 per body (x,y,angle).
     * @property G
     * @type {Array}
     */
    this.G = new Utils.ARRAY_TYPE(6);
    for(var i=0; i&lt;6; i++){
        this.G[i]=0;
    }

    this.offset = 0;

    this.a = 0;
    this.b = 0;
    this.epsilon = 0;
    this.timeStep = 1/60;

    /**
     * Indicates if stiffness or relaxation was changed.
     * @property {Boolean} needsUpdate
     */
    this.needsUpdate = true;

    /**
     * The resulting constraint multiplier from the last solve. This is mostly equivalent to the force produced by the constraint.
     * @property multiplier
     * @type {Number}
     */
    this.multiplier = 0;

    /**
     * Relative velocity.
     * @property {Number} relativeVelocity
     */
    this.relativeVelocity = 0;

    /**
     * Whether this equation is enabled or not. If true, it will be added to the solver.
     * @property {Boolean} enabled
     */
    this.enabled = true;
}
Equation.prototype.constructor = Equation;

/**
 * The default stiffness when creating a new Equation.
 * @static
 * @property {Number} DEFAULT_STIFFNESS
 * @default 1e6
 */
Equation.DEFAULT_STIFFNESS = 1e6;

/**
 * The default relaxation when creating a new Equation.
 * @static
 * @property {Number} DEFAULT_RELAXATION
 * @default 4
 */
Equation.DEFAULT_RELAXATION = 4;

/**
 * Compute SPOOK parameters .a, .b and .epsilon according to the current parameters. See equations 9, 10 and 11 in the &lt;a href=&quot;http://www8.cs.umu.se/kurser/5DV058/VT09/lectures/spooknotes.pdf&quot;&gt;SPOOK notes&lt;/a&gt;.
 * @method update
 */
Equation.prototype.update = function(){
    var k = this.stiffness,
        d = this.relaxation,
        h = this.timeStep;

    this.a = 4.0 / (h * (1 + 4 * d));
    this.b = (4.0 * d) / (1 + 4 * d);
    this.epsilon = 4.0 / (h * h * k * (1 + 4 * d));

    this.needsUpdate = false;
};

/**
 * Multiply a jacobian entry with corresponding positions or velocities
 * @method gmult
 * @return {Number}
 */
Equation.prototype.gmult = function(G,vi,wi,vj,wj){
    return  G[0] * vi[0] +
            G[1] * vi[1] +
            G[2] * wi +
            G[3] * vj[0] +
            G[4] * vj[1] +
            G[5] * wj;
};

/**
 * Computes the RHS of the SPOOK equation
 * @method computeB
 * @return {Number}
 */
Equation.prototype.computeB = function(a,b,h){
    var GW = this.computeGW();
    var Gq = this.computeGq();
    var GiMf = this.computeGiMf();
    return - Gq * a - GW * b - GiMf*h;
};

/**
 * Computes G\*q, where q are the generalized body coordinates
 * @method computeGq
 * @return {Number}
 */
var qi = vec2.create(),
    qj = vec2.create();
Equation.prototype.computeGq = function(){
    var G = this.G,
        bi = this.bodyA,
        bj = this.bodyB,
        xi = bi.position,
        xj = bj.position,
        ai = bi.angle,
        aj = bj.angle;

    return this.gmult(G, qi, ai, qj, aj) + this.offset;
};

/**
 * Computes G\*W, where W are the body velocities
 * @method computeGW
 * @return {Number}
 */
Equation.prototype.computeGW = function(){
    var G = this.G,
        bi = this.bodyA,
        bj = this.bodyB,
        vi = bi.velocity,
        vj = bj.velocity,
        wi = bi.angularVelocity,
        wj = bj.angularVelocity;
    return this.gmult(G,vi,wi,vj,wj) + this.relativeVelocity;
};

/**
 * Computes G\*Wlambda, where W are the body velocities
 * @method computeGWlambda
 * @return {Number}
 */
Equation.prototype.computeGWlambda = function(){
    var G = this.G,
        bi = this.bodyA,
        bj = this.bodyB,
        vi = bi.vlambda,
        vj = bj.vlambda,
        wi = bi.wlambda,
        wj = bj.wlambda;
    return this.gmult(G,vi,wi,vj,wj);
};

/**
 * Computes G\*inv(M)\*f, where M is the mass matrix with diagonal blocks for each body, and f are the forces on the bodies.
 * @method computeGiMf
 * @return {Number}
 */
var iMfi = vec2.create(),
    iMfj = vec2.create();
Equation.prototype.computeGiMf = function(){
    var bi = this.bodyA,
        bj = this.bodyB,
        fi = bi.force,
        ti = bi.angularForce,
        fj = bj.force,
        tj = bj.angularForce,
        invMassi = bi.invMassSolve,
        invMassj = bj.invMassSolve,
        invIi = bi.invInertiaSolve,
        invIj = bj.invInertiaSolve,
        G = this.G;

    vec2.scale(iMfi, fi, invMassi);
    vec2.multiply(iMfi, bi.massMultiplier, iMfi);
    vec2.scale(iMfj, fj,invMassj);
    vec2.multiply(iMfj, bj.massMultiplier, iMfj);

    return this.gmult(G,iMfi,ti*invIi,iMfj,tj*invIj);
};

/**
 * Computes G\*inv(M)\*G&#x27;
 * @method computeGiMGt
 * @return {Number}
 */
Equation.prototype.computeGiMGt = function(){
    var bi = this.bodyA,
        bj = this.bodyB,
        invMassi = bi.invMassSolve,
        invMassj = bj.invMassSolve,
        invIi = bi.invInertiaSolve,
        invIj = bj.invInertiaSolve,
        G = this.G;

    return  G[0] * G[0] * invMassi * bi.massMultiplier[0] +
            G[1] * G[1] * invMassi * bi.massMultiplier[1] +
            G[2] * G[2] *    invIi +
            G[3] * G[3] * invMassj * bj.massMultiplier[0] +
            G[4] * G[4] * invMassj * bj.massMultiplier[1] +
            G[5] * G[5] *    invIj;
};

var addToWlambda_temp = vec2.create(),
    addToWlambda_Gi = vec2.create(),
    addToWlambda_Gj = vec2.create(),
    addToWlambda_ri = vec2.create(),
    addToWlambda_rj = vec2.create(),
    addToWlambda_Mdiag = vec2.create();

/**
 * Add constraint velocity to the bodies.
 * @method addToWlambda
 * @param {Number} deltalambda
 */
Equation.prototype.addToWlambda = function(deltalambda){
    var bi = this.bodyA,
        bj = this.bodyB,
        temp = addToWlambda_temp,
        Gi = addToWlambda_Gi,
        Gj = addToWlambda_Gj,
        ri = addToWlambda_ri,
        rj = addToWlambda_rj,
        invMassi = bi.invMassSolve,
        invMassj = bj.invMassSolve,
        invIi = bi.invInertiaSolve,
        invIj = bj.invInertiaSolve,
        Mdiag = addToWlambda_Mdiag,
        G = this.G;

    Gi[0] = G[0];
    Gi[1] = G[1];
    Gj[0] = G[3];
    Gj[1] = G[4];

    // Add to linear velocity
    // v_lambda += inv(M) * delta_lamba * G
    vec2.scale(temp, Gi, invMassi*deltalambda);
    vec2.multiply(temp, temp, bi.massMultiplier);
    vec2.add( bi.vlambda, bi.vlambda, temp);
    // This impulse is in the offset frame
    // Also add contribution to angular
    //bi.wlambda -= vec2.crossLength(temp,ri);
    bi.wlambda += invIi * G[2] * deltalambda;


    vec2.scale(temp, Gj, invMassj*deltalambda);
    vec2.multiply(temp, temp, bj.massMultiplier);
    vec2.add( bj.vlambda, bj.vlambda, temp);
    //bj.wlambda -= vec2.crossLength(temp,rj);
    bj.wlambda += invIj * G[5] * deltalambda;
};

/**
 * Compute the denominator part of the SPOOK equation: C = G\*inv(M)\*G&#x27; + eps
 * @method computeInvC
 * @param  {Number} eps
 * @return {Number}
 */
Equation.prototype.computeInvC = function(eps){
    return 1.0 / (this.computeGiMGt() + eps);
};

    </pre>
</div>
                    </div>
                </div>
            </div>
        </div>
    </div>
</div>
<script src="../assets/vendor/prettify/prettify-min.js"></script>
<script>prettyPrint();</script>
<script src="../assets/js/yui-prettify.js"></script>
<script src="../assets/../api.js"></script>
<script src="../assets/js/api-filter.js"></script>
<script src="../assets/js/api-list.js"></script>
<script src="../assets/js/api-search.js"></script>
<script src="../assets/js/apidocs.js"></script>
</body>
</html>
